#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <iostream>
#include <ctype.h>

#include <stdio.h>


using namespace cv;

class ImageHandler
{
  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;
  image_transport::Subscriber image_sub_;

  ros::Time time;//定义时间
  ros::Duration duration;
  
  Mat image;
  bool pause, start_record, capture_init_runonce;
  
  VideoWriter writer;
  FILE *fp;

public:
  ImageHandler()
    : it_(nh_)
  {
    // Subscrive to input video feed and publish output video feed
    image_sub_ = it_.subscribe("/dji_sdk/image_raw", 1,
      &ImageHandler::imageCb, this);
      
    pause = false;
    start_record = false;
    capture_init_runonce = true;
    
    time = ros::Time::now();//初始化时间
    duration = ros::Duration(0);
    //fp = fopen("/home/ubuntu/HandleFrame.txt", "w");//打开文件记录
    
    namedWindow( "Video Show", 0 );
    namedWindow( "Video Capture", 0 );
  }

  ~ImageHandler()
  {
    destroyWindow("Video Show");
    destroyWindow("Video Capture");
    fclose(fp);//关闭文件
  }

  void imageCb(const sensor_msgs::ImageConstPtr& msg)
  {
    time = ros::Time::now();//记录接收到图像的时间
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
    
    image = cv_ptr->image;
    
    if(capture_init_runonce)
    {
      capture_init_runonce = !capture_init_runonce;
      char filename[50];
      sprintf(filename, "/home/ubuntu/%.9f.mpg", time.toSec());
      writer.open(filename, CV_FOURCC('D', 'I', 'V', 'X'), 30.0, image.size());
    }
    
    if(start_record && !pause)
    {
      imshow( "Video Capture", image);
      writer << image;
    }
    imshow( "Video Show", image);

    char c = (char)waitKey(1);
    switch(c)
    {
    case 's':
      start_record = true;
      break;
    case 'p':
      if(start_record)
        pause = !pause;
      break;
    default:
    ;
    }
  }
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "image_capture");
  ImageHandler ic;
  ros::spin();
  return 0;
}
